/**
 * @file	gripper_sensor.cpp
 *
 * @brief	sensor of the gripper, publishes the distance to the ground/object.
 * @author	Peter Evers
 */

#include <ros/ros.h>
#include <cmath>
#include <std_msgs/Float32.h>
#include <phidget_ik/phidget_ik.h>
#include "topic/topic.h"

PhidgetIK phidget_ik;

/**
 * Get the sensor value in cm.
 * @param	port of the widget
 * @return	sensor value in cm.
 */
float getSensorValueCm(int port)
{
	float temp = fabs(phidget_ik.getSensorValue(port) - 11);
	if(temp == 0)
		return 30;		// max distance
	return 2076.0/temp;
}

/**
 * Init sensor, return number of inputs.
 * @return number of inputs.
 */
int init()
{
	phidget_ik.init(-1);
	phidget_ik.waitForAttachment(2000);

	// get number of inputs
	int inputs = phidget_ik.getSensorCount();
	if (phidget_ik.getLastError())
	{
		ROS_INFO("Error: %s (%d)", phidget_ik.getErrorDescription(phidget_ik.getLastError()).c_str(), phidget_ik.getLastError());
		return 1;
	}

	return inputs;
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "sensor_gripper");
	ros::NodeHandle nh("~");
	init();

	ros::Publisher distance_pub = nh.advertise<std_msgs::Float32>(Topic::SENSOR_GRIPPER, 10);
	ros::Rate loop_rate(20);
	std_msgs::Float32 msg;

	int port = 7;
	ROS_ASSERT(nh.getParam("port", port));

	while(ros::ok())
	{
		for (int i = 0; i < 8; i++) {
			ROS_INFO("Sensor %i value: %f", i, getSensorValueCm(i));
		}
		ROS_INFO("-----------------");
		msg.data = getSensorValueCm(port);
		distance_pub.publish(msg);

		ros::spinOnce();
		loop_rate.sleep();
	}
}
